#include <MegabriteController.h>
#include <LinearColorMap.h>
#include <BezierInterpolationMap.h>
#include <Average.h>

MegabriteController* megabriteController;
LinearColorMap* colorMap;
Average* valueAverage;
int maxValue;

void setup() {
  Serial.begin(9600);
  valueAverage = newAverageWithSampleSize(1000);
  createLinearColorMap();
  createMegabriteController();
  debugln("initialized");  
  calibrateMaxDistance();
  debugln("calibrated");
}

void calibrateMaxDistance() {
  Average* maxAverage = newAverageWithSampleSize(1000);
  for (int i = 0; i < 5000; i++) {
    maxAverage->addValue(analogRead(0));
    delay(1);
  }
  maxValue = maxAverage->average - 5;
}

void loop() {
  int value = analogRead(0);
  int ave = valueAverage->averageWithValue(value);
  int color = 1000 - map(ave, 10, maxValue, 0, 1000);
  if (color < 0) { color = 0; }
  if (color > 1000) { color = 1000; }
  debug("value: ");
  debug(value);
  debug(" average: ");
  debug(ave);
  debug(" color: ");
  debug(color);
  debug(" max value: ");
  debug(valueAverage->max);
  debug(" max: ");
  debug(maxValue);
  debugln("");
  sendColorValue(color);
}

void sendColorValue(int i) {
    Color color0 = colorMap->getColorForValue(i);
    setMegabriteColor(0, color0);
    setMegabriteColor(1, color0);
    megabriteController->send();
    delay(1);
}

void setMegabriteColor(int megabriteIdx, Color color) {
    Megabrite* megabrite = megabriteController->getMegabriteAtIndex(megabriteIdx);
    megabrite->red = color.red;
    megabrite->green = color.green;
    megabrite->blue = color.blue;
}

void createMegabriteController() {
  megabriteController = newMegabriteController();
  megabriteController->init();
  megabriteController->setNumberOfMegabrites(2);
}

void createLinearColorMap() {
  	colorMap = newLinearColorMap();
	colorMap->addRedPoint(0, 600);
	colorMap->addRedPoint(200, 0);
	colorMap->addRedPoint(400, 0);
	colorMap->addRedPoint(600, 800);
	colorMap->addRedPoint(800, 0);
	colorMap->addRedPoint(1000, 1023);

	colorMap->addGreenPoint(00, 0);
	colorMap->addGreenPoint(200, 0);
	colorMap->addGreenPoint(400, 800);
	colorMap->addGreenPoint(600, 0);
	colorMap->addGreenPoint(800, 600);
	colorMap->addGreenPoint(1000, 1023);

	colorMap->addBluePoint(0, 0);
	colorMap->addBluePoint(200, 1023);
	colorMap->addBluePoint(400, 0);
	colorMap->addBluePoint(600, 600);
	colorMap->addBluePoint(800, 1023);
	colorMap->addBluePoint(1000, 1023);
}

void debug(char* hi) {
  //Serial.print(hi);
}
void debugln(char* hi) {
  //Serial.println(hi);
}

void debug(int hi) {
  //Serial.print(hi);
}
void debugln(int hi) {
  //Serial.println(hi);
}
